Navigator: Fix fixed-wing first order altitude hold #9850
Conversation
…e reference oscillations caused by it in LOITER mode
|
This actually needs a little thought to review. There are a few situations where the position setpoint type and mission_item type are not the same. |
|
I can of course also check for |
|
If you can do it entirely from the position setpoint (ignoring mission_item) it should effectively avoid the edge cases (LOITER_TO_ALT, mission work items, etc). |
|
So you'd also want to use |
|
Yes, but drop the mission_item entirely and do a quick skim of the entire altitude_sp_foh_update() to make sure nothing is dependant on the mission item. This is one of the reasons I wanted to move it to the position controller. Multicopter skips this thing entirely. |
I'll can do this tomorrow if you can't get to it. |
Sure, I guess you know exactly how you'd like to do it. Let me know if i should retest it afterwards then. |
Not really, I just see a number of subtle edge cases that make me uncomfortable. |
|
I played around with this a bit, but there's still another (small) possible hole when the mission item nav_cmd is out of sync with the current position setpoint type. Let's merge this now, but work on moving it to the position controller soon. From there it's much easier to safely handle the altitude ramp or hand off between loiter <-> position without fighting to plug numerous holes. |
i.e. the altitude reference oscillations caused by it in LOITER mode
i.e. the altitude reference oscillations caused by it in LOITER mode
i.e. the altitude reference oscillations caused by it in LOITER mode
Issue
Navigator's fixed-wing first-order altitude hold (FOH) is currently causing altitude reference oscillations when in any LOITER mode. See screenshot below. Not only altitude, but also pitch and throttle can thus oscillate significantly. We observed this during a recent test flight.
Log file from SITL where this can be seen: https://review.px4.io/plot_app?log=f18c45ab-622e-4b1d-9819-03cfb06af2b5 . Here, after t=3:20 the true altitude setpoint is still the same as before t=2:20 (i.e. 630m AMSL) but the FOH logic just wrongly sets it to 660m and adds slight oscillations on top. The exact amount of oscillations depends on waypoint distance, altitude difference etc, but i have seen altitude ref oscillations of up to 30m, resulting in full pitch up/down of the aircraft.
Analysis
See the commit for the location of the code where this is happening. Essentially, every time that the loiter radius of any loiter WP is larger than the acceptance radius calculated from the L1 turn distance, then the Navigator FOH would not consider the waypoint reached and would thus not stop modifying the current altitude setpoint. This leads to the oscillations or offsets in the altitude reference.
Solution
If in any loiter mode, consider the loiter radius times a factor of 1.2 (same as for the waypoint_reached logic here as the acceptance radius. Tested in SITL, which should be sufficient for this case.
@acfloria @ryanjAA @Antiheavy FYI
@dagar Made this PR as a quick hotfix independently of your PR at https://github.com/PX4/Firmware/pull/8883/files which supposedly also fixes this but is much larger and where it is more uncertain when this would be merged.